#include "gait.h"

#include "motor_device.h"
#include "usr_conf.h"
#include "cmsis_os.h"
#include "dt7.h"
#include "math.h"

void Stand_Body(void)
{
	Leg_SetPos(LEG_INDEX_LB, 0, 3.0f*RC_CtrlData.rc.RightY + LB.zero_y);
	Leg_SetPos(LEG_INDEX_LF, 0, 3.0f*RC_CtrlData.rc.RightY + LF.zero_y);
	Leg_SetPos(LEG_INDEX_RB, 0, 3.0f*RC_CtrlData.rc.RightY + RB.zero_y);
	Leg_SetPos(LEG_INDEX_RF, 0, 3.0f*RC_CtrlData.rc.RightY + RF.zero_y);
}


void Walk_Body(Gait_e gait, float forward_spd, float rotate_spd, float phase)
{
	//LF.zero_x = RF.zero_x = LB.zero_x = RB.zero_x = (-forward_spd*1.7);//身体前倾

	LF.Ax = RF.Ax = LB.Ax = RB.Ax = forward_spd*6;
	rotate_spd*=6;
	LF.Ax -= rotate_spd;
	LB.Ax -= rotate_spd;
	RF.Ax += rotate_spd;
	RB.Ax += rotate_spd;
	
	LF.zero_x = (+LF.Ax*0.1);
	RF.zero_x = (+RF.Ax*0.1);
	LB.zero_x = (+LB.Ax*0.1);
	RB.zero_x = (+RB.Ax*0.1);//身体前倾
	
	

	LF.Ay = RF.Ay = LB.Ay = RB.Ay = 4;
	if(forward_spd > 0)
	{
//		Trot_Leg(gait, LEG_INDEX_LF, phase+PI/2);
//		Trot_Leg(gait, LEG_INDEX_RF, phase+PI*3/2);
//		Trot_Leg(gait, LEG_INDEX_LB, phase+PI);
//		Trot_Leg(gait, LEG_INDEX_RB, phase);
		Trot_Leg(gait, LEG_INDEX_LF, phase);
		Trot_Leg(gait, LEG_INDEX_RF, phase+PI);
		Trot_Leg(gait, LEG_INDEX_LB, phase+PI/2);
		Trot_Leg(gait, LEG_INDEX_RB, phase+PI*3/2);
	} else {
		
		Trot_Leg(gait, LEG_INDEX_LF, phase+PI*3/2);
		Trot_Leg(gait, LEG_INDEX_RF, phase+PI/2);
		Trot_Leg(gait, LEG_INDEX_LB, phase+PI);
		Trot_Leg(gait, LEG_INDEX_RB, phase);
	}
}

void Trot_Body(Gait_e gait, float forward_spd, float rotate_spd, float phase)
{
	//LF.zero_x = RF.zero_x = LB.zero_x = RB.zero_x = (-forward_spd*1.7);//身体前倾

	LF.Ax = RF.Ax = LB.Ax = RB.Ax = forward_spd*2;
	LF.Ax -= rotate_spd;
	LB.Ax -= rotate_spd;
	RF.Ax += rotate_spd;
	RB.Ax += rotate_spd;
	
	LF.zero_x = (-LF.Ax*0.8);
	RF.zero_x = (-RF.Ax*0.8);
	LB.zero_x = (-LB.Ax*0.8);
	RB.zero_x = (-RB.Ax*0.8);//身体前倾
	
	LF.Ay = RF.Ay = LB.Ay = RB.Ay = 1.4;
	
	Trot_Leg(gait, LEG_INDEX_LF, phase);
	Trot_Leg(gait, LEG_INDEX_RF, phase+PI);
	Trot_Leg(gait, LEG_INDEX_LB, phase+PI);
	Trot_Leg(gait, LEG_INDEX_RB, phase);
}

#define YH 1
#define YL -0.3
#define XH 0.5
#define XL -1.5

void Trot_Leg(Gait_e gait, Leg_index_e leg_index, double phase)
{
			float switch_time = PI/4;
//	static double leg_len1 = 4, leg_len2 = 9;
	Leg_t* in = &leg[leg_index];
	float Ax = in->Ax, Ay = in->Ay, zero_x = in->zero_x, zero_y = in->zero_y;
	static float x = 0;
	static float y = 0;
	if (phase > 2*PI) phase = phase - 2*PI;


	if(phase > PI) in->Ay *= 0;
    /* Choosing Gait */
	switch (gait)
    {
        
    case GAIT_DIY :
        
        if (phase<=PI)
        {
            x = zero_x + Ax * ((XH-XL)/2.0 *( - cos(phase)));
            y = zero_y - Ay*YH*sin(phase);
        } else if ((phase>PI) && (phase<=5.0/3*PI))
        {
            x = zero_x + Ax * ((XH-XL)/2 * cos((phase-PI)*3.0/2.0));
            y = zero_y - Ay*YL*sin(3.0/4.0*(phase-PI));
        } else
        {
            x = zero_x + Ax * XL;
            y = zero_y - Ay*((-YL*sin(3.0f/2.0f*(phase-2.0f*PI))));
        }
        
        break;
        
    case GAIT_CIRCLE :
				if(phase > PI) Ay*=0.8;
        x = zero_x + cos(phase)*Ax;
        y = zero_y + sin(phase)*Ay;
        break;
        
    case GAIT_WALK_HALFCIRCLE :
				if(phase < switch_time) 
        {
//					x = zero_x - cos(phase*4)*Ax;
//					y = zero_y - sin(phase*4)*Ay;
					if(0 < phase && phase <= switch_time/4)
					{
						x = zero_x -Ax;
						y = zero_y - (phase/(switch_time/4))*Ay;
					} else if(switch_time/4 < phase && phase <= switch_time*3/4)
					{
						
						x = zero_x + (4*Ax/switch_time)*(phase-(switch_time/4)) - Ax;
						y = zero_y - Ay;
					} else if(switch_time*3/4 < phase && phase <= switch_time)
					{
						
						x = zero_x + Ax;
						y = zero_y - Ay + ((phase-switch_time*3/4)/(switch_time/4))*Ay;
					}
				} else {
					y = zero_y;
					x = zero_x - ((2*Ax)/(2*PI-switch_time))*(phase-2*PI)-Ax;
				}
        break;
    default:
        break;
    }
    
    /* Drive Leg */
    Leg_SetPos(leg_index, x, y);
	/*
	double theta = 0.3927 
	- 0.5 * sin(phase - PI / 2)
	;
	double alpha = PI-(0.7854 
	+ 0.5 * sin(phase + PI)
	);
	
	Leg_SetPos(leg_index,
	zero_x + (leg_len1*sin(theta)-leg_len2*cos(alpha-(PI/2 - theta))) * Ax
	,zero_y + (leg_len1*cos(theta)+leg_len2*sin(alpha-(PI/2 - theta))) * Ay);
	*/
}

float Quadraped_GetPhase(int period)
{
	float t=0.0;
	t = (osKernelSysTick()%(int)(period));
	float phase = t/period*2*PI;
	return phase;
}
